#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stdbool.h>
#include <assert.h>
#include <stdlib.h>
#include "aoi.h"

#define INVALID_ID (~0)
#define PRE_ALLOC 16
#define DIST2(p1, p2) ((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1]) + (p1[2] - p2[2]) * (p1[2] - p2[2]))
#define MODE_WATCHER 1
#define MODE_MARKER 2
#define MODE_MOVE 4
#define MODE_DROP 8

static struct object *
new_object(struct aoi_space *space, uint32_t id)
{
	struct object *obj = space->alloc(space->alloc_ud, NULL, sizeof(*obj));
	obj->ref = 1;
	obj->id = id;
	obj->mode = 0;
	obj->last[0] = 0.0f;
	obj->last[1] = 0.0f;
	obj->last[2] = 0.0f;
	obj->last_tpos[0] = 0.0f;
	obj->last_tpos[1] = 0.0f;
	obj->last_tpos[2] = 0.0f;
	obj->tposition[0] = 0.0f;
	obj->tposition[1] = 0.0f;
	obj->tposition[2] = 0.0f;
	obj->rtype = 0;
	obj->aoiCount = 0;
	return obj;
}

static inline struct map_slot *
mainposition(struct map *m, uint32_t id)
{
	uint32_t hash = id & (m->size - 1);
	return &m->slot[hash];
}

static void rehash(struct aoi_space *space, struct map *m);

static void
map_insert(struct aoi_space *space, struct map *m, uint32_t id, struct object *obj)
{
	struct map_slot *s = mainposition(m, id);
	if (s->id == INVALID_ID)
	{
		s->id = id;
		s->obj = obj;
		return;
	}
	if (mainposition(m, s->id) != s)
	{
		struct map_slot *last = mainposition(m, s->id);
		while (last->next != s - m->slot)
		{
			assert(last->next >= 0);
			last = &m->slot[last->next];
		}
		uint32_t temp_id = s->id;
		struct object *temp_obj = s->obj;
		last->next = s->next;
		s->id = id;
		s->obj = obj;
		s->next = -1;
		if (temp_obj)
		{
			map_insert(space, m, temp_id, temp_obj);
		}
		return;
	}
	while (m->lastfree >= 0)
	{
		struct map_slot *temp = &m->slot[m->lastfree--];
		if (temp->id == INVALID_ID)
		{
			temp->id = id;
			temp->obj = obj;
			temp->next = s->next;
			s->next = (int)(temp - m->slot);
			return;
		}
	}
	rehash(space, m);
	map_insert(space, m, id, obj);
}

static void
rehash(struct aoi_space *space, struct map *m)
{
	struct map_slot *old_slot = m->slot;
	int old_size = m->size;
	m->size = 2 * old_size;
	m->lastfree = m->size - 1;
	m->slot = space->alloc(space->alloc_ud, NULL, m->size * sizeof(struct map_slot));
	int i;
	for (i = 0; i < m->size; i++)
	{
		struct map_slot *s = &m->slot[i];
		s->id = INVALID_ID;
		s->obj = NULL;
		s->next = -1;
	}
	for (i = 0; i < old_size; i++)
	{
		struct map_slot *s = &old_slot[i];
		if (s->obj)
		{
			map_insert(space, m, s->id, s->obj);
		}
	}
	space->alloc(space->alloc_ud, old_slot, old_size * sizeof(struct map_slot));
}

static struct object *
map_query(struct aoi_space *space, struct map *m, uint32_t id)
{
	struct map_slot *s = mainposition(m, id);
	for (;;)
	{
		if (s->id == id)
		{
			if (s->obj == NULL)
			{
				s->obj = new_object(space, id);
			}
			return s->obj;
		}
		if (s->next < 0)
		{
			break;
		}
		s = &m->slot[s->next];
	}
	struct object *obj = new_object(space, id);
	map_insert(space, m, id, obj);
	return obj;
}

static void
map_foreach(struct map *m, void (*func)(void *ud, struct object *obj), void *ud)
{
	int i;
	for (i = 0; i < m->size; i++)
	{
		if (m->slot[i].obj)
		{
			func(ud, m->slot[i].obj);
		}
	}
}

static struct object *
map_drop(struct map *m, uint32_t id)
{
	uint32_t hash = id & (m->size - 1);
	struct map_slot *s = &m->slot[hash];
	for (;;)
	{
		if (s->id == id)
		{
			struct object *obj = s->obj;
			s->obj = NULL;
			return obj;
		}
		if (s->next < 0)
		{
			return NULL;
		}
		s = &m->slot[s->next];
	}
}

static void
map_delete(struct aoi_space *space, struct map *m)
{
	space->alloc(space->alloc_ud, m->slot, m->size * sizeof(struct map_slot));
	space->alloc(space->alloc_ud, m, sizeof(*m));
}

static struct map *
map_new(struct aoi_space *space)
{
	int i;
	struct map *m = space->alloc(space->alloc_ud, NULL, sizeof(*m));
	m->size = PRE_ALLOC;
	m->lastfree = PRE_ALLOC - 1;
	m->slot = space->alloc(space->alloc_ud, NULL, m->size * sizeof(struct map_slot));
	for (i = 0; i < m->size; i++)
	{
		struct map_slot *s = &m->slot[i];
		s->id = INVALID_ID;
		s->obj = NULL;
		s->next = -1;
	}
	return m;
}

inline static void
grab_object(struct object *obj)
{
	++obj->ref;
}

static void
delete_object(void *s, struct object *obj)
{
	struct aoi_space *space = s;
	space->alloc(space->alloc_ud, obj, sizeof(*obj));
}

inline static void
drop_object(struct aoi_space *space, struct object *obj)
{
	--obj->ref;
	if (obj->ref <= 0)
	{
		map_drop(space->object, obj->id);
		delete_object(space, obj);
	}
}

static struct object_set *
set_new(struct aoi_space *space)
{
	struct object_set *set = space->alloc(space->alloc_ud, NULL, sizeof(*set));
	set->cap = PRE_ALLOC;
	set->number = 0;
	set->slot = space->alloc(space->alloc_ud, NULL, set->cap * sizeof(struct object *));
	return set;
}

struct aoi_space *
aoi_create(aoi_Alloc alloc, void *ud)
{
	struct aoi_space *space = alloc(ud, NULL, sizeof(*space));
	space->alloc = alloc;
	space->alloc_ud = ud;
	space->object = map_new(space);
	space->watcher_static = set_new(space);
	space->marker_static = set_new(space);
	space->watcher_move = set_new(space);
	space->marker_move = set_new(space);
	space->hot = NULL;
	space->aoi_radis2 = 0;
	return space;
}

static void
delete_pair_list(struct aoi_space *space)
{
	struct pair_list *p = space->hot;
	while (p)
	{
		struct pair_list *next = p->next;
		space->alloc(space->alloc_ud, p, sizeof(*p));
		p = next;
	}
}

static void
delete_set(struct aoi_space *space, struct object_set *set)
{
	if (set->slot)
	{
		space->alloc(space->alloc_ud, set->slot, sizeof(struct object *) * set->cap);
	}
	space->alloc(space->alloc_ud, set, sizeof(*set));
}

void aoi_release(struct aoi_space *space)
{
	map_foreach(space->object, delete_object, space);
	map_delete(space, space->object);
	delete_pair_list(space);
	delete_set(space, space->watcher_static);
	delete_set(space, space->marker_static);
	delete_set(space, space->watcher_move);
	delete_set(space, space->marker_move);
	space->alloc(space->alloc_ud, space, sizeof(*space));
}

inline static void
copy_position(float des[3], float src[3])
{
	des[0] = src[0];
	des[1] = src[1];
	des[2] = src[2];
}

inline static bool
zero_position(float p[3])
{
	return p[0] == 0.0f && p[1] == 0.0f && p[2] == 0.0f;
}

static bool
change_mode(struct object *obj, bool set_watcher, bool set_marker)
{
	bool change = false;
	if (obj->mode == 0)
	{
		if (set_watcher)
		{
			obj->mode = MODE_WATCHER;
		}
		if (set_marker)
		{
			obj->mode |= MODE_MARKER;
		}
		return true;
	}
	if (set_watcher)
	{
		if (!(obj->mode & MODE_WATCHER))
		{
			obj->mode |= MODE_WATCHER;
			change = true;
		}
	}
	else
	{
		if (obj->mode & MODE_WATCHER)
		{
			obj->mode &= ~MODE_WATCHER;
			change = true;
		}
	}
	if (set_marker)
	{
		if (!(obj->mode & MODE_MARKER))
		{
			obj->mode |= MODE_MARKER;
			change = true;
		}
	}
	else
	{
		if (obj->mode & MODE_MARKER)
		{
			obj->mode &= ~MODE_MARKER;
			change = true;
		}
	}
	return change;
}

inline static bool
is_near(float p1[3], float p2[3])
{
	return DIST2(p1, p2) < 0;
}

inline static float
dist2(struct object *p1, struct object *p2)
{
	float d = DIST2(p1->position, p2->position);
	return d;
}

static void
grap_move(struct object *obj)
{
	obj->mode |= MODE_MOVE;
}

//static size_t
static void
make_unique_int64(size_t l_id, size_t r_id, char* key, int keylen)
{
	snprintf(key, keylen , "%ld%ld", l_id, r_id);
	//return (l_id << 32) | (r_id & 0xffffffff);
}

void aoi_update(struct aoi_space *space, uint32_t id, const char *modestring,
				uint32_t rtype, float pos[3], float tpos[3], aoi_Callback cb, void *ud)
{
	struct object *obj = map_query(space, space->object, id);
	int i;
	bool set_watcher = false;
	bool set_marker = false;

	for (i = 0; modestring[i]; ++i)
	{
		char m = modestring[i];
		switch (m)
		{
		case 'w':
			set_watcher = true;
			break;
		case 'm':
			set_marker = true;
			break;
		case 'd':
			if (!(obj->mode & MODE_DROP))
			{
				obj->mode = MODE_DROP;
				drop_object(space, obj);
			}
			return;
		}
	}

	if (obj->mode & MODE_DROP)
	{
		obj->mode &= ~MODE_DROP;
		grab_object(obj);
	}

	bool changed = change_mode(obj, set_watcher, set_marker);

	copy_position(obj->position, pos);
	copy_position(obj->last_tpos, obj->tposition);
	copy_position(obj->tposition, tpos);
	obj->rtype = rtype;
	if (changed || !is_near(pos, obj->last))
	{
		// new object or change object mode
		// or position changed
		copy_position(obj->last, pos);
		grap_move(obj);
	}
}

static void
drop_pair(struct aoi_space *space, struct pair_list *p)
{
	// remove from hash
	HASH_DEL(space->pair_hash, p);
	p->watcher->aoiCount--;
	drop_object(space, p->watcher);
	drop_object(space, p->marker);
	space->alloc(space->alloc_ud, p, sizeof(*p));
}

static void
flush_pair(struct aoi_space *space, aoi_Callback cb, void *ud)
{
	struct pair_list **last = &(space->hot);
	struct pair_list *p = *last;
	while (p)
	{
		struct pair_list *next = p->next;
		if (p->watcher->mode & MODE_DROP || p->marker->mode & MODE_DROP)
		{
			if (p->marker->mode & MODE_DROP) // 只有marker 主动DROP才需要回调, watcher DROP不需要通知marker
				cb(ud, space->aoiMapId, p->watcher->id, p->marker->id, "d", p->marker->position, p->marker->tposition, p->marker->rtype);
			drop_pair(space, p);
			*last = next;
		}
		else if (p->marker->mode & MODE_MOVE)
		{
			if (dist2(p->watcher, p->marker) > space->aoi_radis2)
			{
				// 超出了AOI范围
				cb(ud, space->aoiMapId, p->watcher->id, p->marker->id, "d", p->marker->position, p->marker->tposition, p->marker->rtype);
				drop_pair(space, p);
				*last = next;
			}
			else
			{
				if (DIST2(p->marker->last_tpos, p->marker->tposition) > 0)
				{
					// 正常移动(或者新的对象组)
					cb(ud, space->aoiMapId, p->watcher->id, p->marker->id, "m", p->marker->position, p->marker->tposition, p->marker->rtype);
				}
				last = &(p->next);
			}
		}
		else
		{
			float distance2 = dist2(p->watcher, p->marker);
			if (distance2 > space->aoi_radis2)
			{
				// 超过pair的距离
				cb(ud, space->aoiMapId, p->watcher->id, p->marker->id, "d", p->marker->position, p->marker->tposition, p->marker->rtype);
				drop_pair(space, p);
				*last = next;
			}
			else
				last = &(p->next);
		}
		p = next;
	}
}

static void
set_push_back(struct aoi_space *space, struct object_set *set, struct object *obj)
{
	if (set->number >= set->cap)
	{
		int cap = set->cap * 2;
		void *tmp = set->slot;
		set->slot = space->alloc(space->alloc_ud, NULL, cap * sizeof(struct object *));
		memcpy(set->slot, tmp, set->cap * sizeof(struct object *));
		space->alloc(space->alloc_ud, tmp, set->cap * sizeof(struct object *));
		set->cap = cap;
	}
	set->slot[set->number] = obj;
	++set->number;
}

static void
set_push(void *s, struct object *obj)
{
	struct aoi_space *space = s;
	int mode = obj->mode;
	if (mode & MODE_WATCHER)
	{
		if (mode & MODE_MOVE)
		{
			set_push_back(space, space->watcher_move, obj);
			obj->mode &= ~MODE_MOVE;
		}
		else
		{
			set_push_back(space, space->watcher_static, obj);
		}
	}
	if (mode & MODE_MARKER)
	{
		if (mode & MODE_MOVE)
		{
			set_push_back(space, space->marker_move, obj);
			obj->mode &= ~MODE_MOVE;
		}
		else
		{
			set_push_back(space, space->marker_static, obj);
		}
	}
}

static void
gen_pair(struct aoi_space *space, struct object *watcher, struct object *marker, aoi_Callback cb, void *ud)
{
	if (watcher == marker || watcher->aoiCount >= space->maxAoiCount)
		return;
	struct pair_list *find_pair_hash = NULL;
	char key_str[PAIR_KEY_LEN] = {0};
	make_unique_int64(watcher->id, marker->id, key_str, sizeof(key_str));
	HASH_FIND_STR(space->pair_hash, key_str, find_pair_hash);
	if (find_pair_hash != NULL)
		return;
	if (dist2(watcher, marker) > space->aoi_radis2)
		return;

	// 小于pair距离,生成pair_list
	struct pair_list *p = space->alloc(space->alloc_ud, NULL, sizeof(*p));
	memcpy(p->key, key_str, PAIR_KEY_LEN);
	p->watcher = watcher;
	grab_object(watcher);
	p->marker = marker;
	grab_object(marker);
	p->next = space->hot;
	space->hot = p;

	watcher->aoiCount++;

	// 通知 move
	cb(ud, space->aoiMapId, p->watcher->id, p->marker->id, "m", p->marker->position, p->marker->tposition, p->marker->rtype);

	// 加入 hash
	HASH_ADD_STR(space->pair_hash, key, p);
}

static void
gen_pair_list(struct aoi_space *space, struct object_set *watcher, struct object_set *marker, aoi_Callback cb, void *ud)
{
	int i, j;
	for (i = 0; i < watcher->number; i++)
	{
		for (j = 0; j < marker->number; j++)
		{
			gen_pair(space, watcher->slot[i], marker->slot[j], cb, ud);
		}
	}
}

void aoi_message(struct aoi_space *space, aoi_Callback cb, void *ud)
{
	flush_pair(space, cb, ud);
	space->watcher_static->number = 0;
	space->watcher_move->number = 0;
	space->marker_static->number = 0;
	space->marker_move->number = 0;
	map_foreach(space->object, set_push, space);
	gen_pair_list(space, space->watcher_static, space->marker_move, cb, ud);
	gen_pair_list(space, space->watcher_move, space->marker_static, cb, ud);
	gen_pair_list(space, space->watcher_move, space->marker_move, cb, ud);
}

static void *
default_alloc(void *ud, void *ptr, size_t sz)
{
	if (ptr == NULL)
	{
		void *p = calloc(1, sz);
		return p;
	}
	free(ptr);
	return NULL;
}

struct aoi_space *
aoi_new(float radius)
{
	struct aoi_space *as = aoi_create(default_alloc, NULL);
	if(as != NULL)
		as->aoi_radis2 = ( radius * radius );
	return as;
}
